#ifndef __OBS_H__
#define __OBS_H_

#include "exp_node.h"

// 防碰撞参数
extern double epsilon;       // 防碰撞系数
extern double keppa_colli;   // 势场系数
extern double r_a;           // 势场系数
extern double r_edge;        // 势场边界
extern double L_detect;      // 探测范围半径
extern double L_min;         // 障碍物半径，也是objects.yaml中设置的z参数
extern double K_outer;
extern double K_inner;
extern Eigen::MatrixXd feedback_temp;       

double cal_distance_ij(geometry_msgs::PoseStamped p1, geometry_msgs::PoseStamped p2);

double cal_distance_ik(geometry_msgs::PoseStamped p1, Eigen::MatrixXd p2);

Eigen::VectorXd cal_distance_vec(geometry_msgs::PoseStamped *position_agent, int index);

Eigen::VectorXd cal_weight_vec(geometry_msgs::PoseStamped *position_agent, Eigen::MatrixXd position_obs, int index);

Eigen::VectorXd avoid_feedback(geometry_msgs::PoseStamped *position_agent, Eigen::MatrixXd position_obs, int index);

#endif